#include "kernel.h"
#include "MSE_DBUS.h"
#include "MSE_ROOT_INIT.h"

void kernel_run()
{

	static uint16_t MSE_V_BOTTOM_SPEED_X = 1230;
	static uint16_t MSE_V_BOTTOM_SPEED_Y = 830;
	static uint8_t  MSE_V_BOTTOM_SPEED_W = 120;
	
	int8_t MSE_V_BOTTOM_VX = MSE_V_DBUS.KeyBoard.W  - MSE_V_DBUS.KeyBoard.S;
	int8_t MSE_V_BOTTOM_VY = MSE_V_DBUS.KeyBoard.D  - MSE_V_DBUS.KeyBoard.A;
	int16_t MSE_V_BOTTOM_VW = MSE_V_DBUS.Mouse.X_Flt;
	
    static uint16_t MSE_V_HEAD_SPEED_LIFT   = 200;
	static uint16_t MSE_V_HEAD_SPEED_HORI   = 260;
	static uint16_t MSE_V_HEAD_SPEED_SWAY   = 260;
	
    int8_t MSE_V_HEAD_LIFT = MSE_V_DBUS.KeyBoard.Q  - MSE_V_DBUS.KeyBoard.E;
	int8_t MSE_V_HEAD_HORI = MSE_V_DBUS.KeyBoard.W  - MSE_V_DBUS.KeyBoard.S;
	int8_t MSE_V_HEAD_SWAY = MSE_V_DBUS.KeyBoard.D  - MSE_V_DBUS.KeyBoard.A;
	
	static uint16_t MSE_V_HEAD_SPEED_P   = 300;
	static uint16_t MSE_V_HEAD_SPEED_R   = 300;
	static uint16_t MSE_V_HEAD_SPEED_F   = 300;
	static float MSE_V_HEAD_SPEED_Y   = 1.1f;
	
	int8_t MSE_V_HEAD_P = MSE_V_DBUS.Mouse.Z_Flt;
	int8_t MSE_V_HEAD_F = MSE_V_DBUS.KeyBoard.Z - MSE_V_DBUS.KeyBoard.X;
	int8_t MSE_V_HEAD_R = MSE_V_DBUS.KeyBoard.F;
	int8_t MSE_V_HEAD_Y = MSE_V_DBUS.Mouse.X_Flt;
	
	   if(MSE_V_DBUS.KeyBoard.Shift == 1)
	{
		
			MSE_V_CONTAL.BOTTOM.VX = MSE_V_BOTTOM_SPEED_X * MSE_V_BOTTOM_VX;
			MSE_V_CONTAL.BOTTOM.VY = MSE_V_BOTTOM_SPEED_Y * MSE_V_BOTTOM_VY;
			MSE_V_CONTAL.BOTTOM.VW = MSE_V_BOTTOM_SPEED_W * MSE_V_BOTTOM_VW;

	}
	
	if(MSE_V_DBUS.KeyBoard.Shift == 0)
	{
		
		
			MSE_V_CONTAL.HEAD.LIFT += MSE_V_HEAD_SPEED_LIFT * MSE_V_HEAD_LIFT;   //QE
			MSE_V_CONTAL.HEAD.HORIZONTAL += MSE_V_HEAD_SPEED_HORI * MSE_V_HEAD_HORI;//WS
			MSE_V_CONTAL.HEAD.SWAY += MSE_V_HEAD_SPEED_SWAY * MSE_V_HEAD_SWAY;//AD
		
			MSE_V_CONTAL.HEAD.Pitch += MSE_V_HEAD_SPEED_P * MSE_V_HEAD_P;//M Z
			MSE_V_CONTAL.HEAD.A_CUP_PUMP += MSE_V_HEAD_SPEED_F * MSE_V_HEAD_F;//ZX
			MSE_V_CONTAL.HEAD.R_CUP_PUMP += MSE_V_HEAD_SPEED_R * MSE_V_HEAD_R;//F
			MSE_V_CONTAL.HEAD.Yaw -= MSE_V_HEAD_SPEED_Y * MSE_V_HEAD_Y;//M X
		
		
		
	        MSE_V_CONTAL.BOTTOM.VX = 0;
	        MSE_V_CONTAL.BOTTOM.VY = 0;
	        MSE_V_CONTAL.BOTTOM.VW = 0;
	        MSE_V_CONTAL.BOTTOM.VR = 0;
	        MSE_V_CONTAL.BOTTOM.VZ = 0;

	}
	
	MSE_V_CONTAL.HEAD.CUP_PUMP = MSE_V_DBUS.Mouse.R_PressTime;
	

}